#include <fstream>
#include <iostream>

#include <sstream>
using namespace std;

#include "slamBase.hpp"

// 给定index，读取一帧数据
FRAME readFrame(int index, ParameterReader &pr);
// 度量运动的大小
double normofTransform(cv::Mat rvec, cv::Mat tvec);

int main(int argc, char **argv)
{
    ParameterReader pr;
    int startIndex = pr.config["start_index"].as<int>();
    int endIndex = pr.config["end_index"].as<int>();

    // initialize
    cout << "Initializing ..." << endl;
    int currIndex = startIndex;                 // 当前索引为currIndex
    FRAME lastFrame = readFrame(currIndex, pr); // 上一帧数据
    // 我们总是在比较currFrame和lastFrame
    string detector = pr.config["detector"].as<string>();
    string descriptor = pr.config["descriptor"].as<string>();
    CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
    computeKeyPointsAndDesp(lastFrame, detector, descriptor);
    PointCloud::Ptr cloud = image2PointCloud(lastFrame.rgb, lastFrame.depth, camera);

    pcl::visualization::CloudViewer viewer("viewer");

    // 是否显示点云
    bool visualize = pr.config["visualize_pointcloud"].as<bool>();

    int min_inliers = pr.config["min_inliers"].as<int>();
    double max_norm = pr.config["max_norm"].as<double>();

    for (currIndex = startIndex + 1; currIndex < endIndex; currIndex++)
    {
        cout << "Reading files " << currIndex << endl;
        FRAME currFrame = readFrame(currIndex, pr); // 读取currFrame
        computeKeyPointsAndDesp(currFrame, detector, descriptor);
        // 比较currFrame 和 lastFrame
        RESULT_OF_PNP result = estimateMotion(lastFrame, currFrame, camera, pr);
        if (result.inliers < min_inliers) // inliers不够，放弃该帧
            continue;
        // 计算运动范围是否太大
        double norm = normofTransform(result.rvec, result.tvec);
        cout << "norm = " << norm << endl;
        if (norm >= max_norm)
            continue;
        Eigen::Isometry3d T = cvMat2Eigen(result.rvec, result.tvec);
        cout << "T=" << T.matrix() << endl;

        cloud = joinPointCloud(cloud, currFrame, T, camera);

        if (visualize == true)
            viewer.showCloud(cloud);

        lastFrame = currFrame;
    }

    pcl::io::savePCDFile("../data/results/result.pcd", *cloud);
    return 0;
}

FRAME readFrame(int index, ParameterReader &pd)
{
    FRAME f;
    string rgbDir = pd.config["rgb_dir"].as<string>();
    string depthDir = pd.config["depth_dir"].as<string>();

    string rgbExt = pd.config["rgb_extension"].as<string>();
    string depthExt = pd.config["depth_extension"].as<string>();

    stringstream ss;
    ss << rgbDir << index << rgbExt;
    string filename;
    ss >> filename;
    f.rgb = cv::imread(filename);

    ss.clear();
    filename.clear();
    ss << depthDir << index << depthExt;
    ss >> filename;

    f.depth = cv::imread(filename, -1);
    return f;
}

double normofTransform(cv::Mat rvec, cv::Mat tvec)
{
    return fabs(min(cv::norm(rvec), 2 * M_PI - cv::norm(rvec))) + fabs(cv::norm(tvec));
}